/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
/**
 *
 * $Id: TGIMI_PROTOCOL_PTU_POSITION_COMMAND.h,v 1.7 2007-03-29 14:00:26 amaula Exp $
 *
 */
#ifndef TGIMI_PROTOCOL_PTU_POSITION_COMMAND_H_
#define TGIMI_PROTOCOL_PTU_POSITION_COMMAND_H_

#include <inttypes.h>
#include <math.h>

struct TGIMI_PROTOCOL_PTU_POSITION_COMMAND
{
public:
  
  TGIMI_PROTOCOL_PTU_POSITION_COMMAND() 
    : pan(127),
      tilt(127) {}
  
  TGIMI_PROTOCOL_PTU_POSITION_COMMAND(const int32_t aPan, 
				      const int32_t aTilt)
    : pan(aPan>254?254:aPan<0?0:aPan),
      tilt(aTilt>254?254:aTilt<0?0:aTilt) {}  
  
  TGIMI_PROTOCOL_PTU_POSITION_COMMAND(const double aPan, 
				      const double aTilt)
    : pan((int32_t)(((aPan/(M_PI/2))*127.00)+127.00)),
      tilt((int32_t)(((aTilt/(M_PI/2))*127.00)+127.00)) {}  

  ~TGIMI_PROTOCOL_PTU_POSITION_COMMAND() {}
  
  
  /**
   * Set the PAN raw value. If the value is out of range,
   * it will be truncated.
   *
   * @param[in] aPanRaw         Raw value to set. Valid
   *                            range is 0-254.
   */
  void SetPanRaw(const int32_t aPanRaw) {
    pan = aPanRaw;
    if (pan > 254) pan = 254;
    else if (pan < 0) pan = 0;
  }
  
  
  /**
   * Set the TILT raw value. If the value is out of range,
   * it will be truncated.
   *
   * @param[in] aTiltRaw         Raw value to set. Valid
   *                             range is 0-254.
   */  
  void SetTiltRaw(const int32_t aTiltRaw) {
    tilt = aTiltRaw;
    if (tilt > 254) tilt = 254;
    else if (tilt < 0) tilt = 0;
  }

 
  /**
   * Set the PAN of PTU.
   * 
   * @param[in] aPan            Angle to Pan (rads)
   */
  void SetPan(const double aPan) {
    // 0 rads = 127 raw
    // M_PI/2 rads = 254 raw
    // -M_PI/2 rads = 0 raw
    pan = (int32_t)(((aPan/(M_PI/2))*127.00)+127.00);
  }
  
  /**
   * Set the TILT of PTU.
   * 
   * @param[in] aTilt            Angle to Tilt (rads)
   */
  void SetTilt(const double aTilt) {
    // 0 rads = 127 raw
    // M_PI/2 rads = 254 raw
    // -M_PI/2 rads = 0 raw
    tilt = (int32_t)(((aTilt/(M_PI/2))*127.00)+127.00);
  }
    
public:
  int32_t pan;
  int32_t tilt;
  
};
#endif

